#include "slam.h"
namespace cartographer {
namespace slam {
Slam::Slam() {
  map_builder_ptr_ = std::make_unique<mapping::MapBuilder>();
  node_handle_ptr_ = std::make_unique<ros::NodeHandle>();
  command_srv_ = node_handle_ptr_->advertiseService(
      "command", &Slam::SetCommandCallBack, this);
  submaps_publisher_ =
      node_handle_ptr_->advertise<nav_msgs::OccupancyGrid>("map", 100);
  submap_poses_publisher_ =
      node_handle_ptr_->advertise<nav_msgs::Path>("submap_poses", 100);
  node_poses_publisher_ =
      node_handle_ptr_->advertise<nav_msgs::Path>("node_poses", 100);
  submap_pose_publisher_ = node_handle_ptr_->advertise<geometry_msgs::PoseStamped>("submap_pose", 100);
  submap_min_pose_publisher_ = node_handle_ptr_->advertise<geometry_msgs::PoseStamped>("submap_min_pose", 100);
  submap_max_pose_publisher_ = node_handle_ptr_->advertise<geometry_msgs::PoseStamped>("submap_max_pose", 100);
};

Slam::~Slam(){};

bool Slam::SetCommandCallBack(Command::Request& req, Command::Response& res) {
  if (req.command == "load") {
    std::string filename = std::string(kMapsDirectory) +
                           std::to_string(req.filename) + ".pbstream";
    map_builder_ptr_->LoadStateFromFile(filename, true);      
    PublishNodePoses();
    PublishSubmapPoses();
    PublishSubmaps();
  }
  return true;
};
void Slam::PublishNodePoses() {
  mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses =
      map_builder_ptr_->pose_graph()->GetTrajectoryNodePoses();
  nav_msgs::Path path;
  path.header.stamp = ros::Time::now();
  path.header.frame_id = "map";
  for (auto&& node_pose : node_poses) {
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time::now();
    pose_stamped.header.frame_id = "map";
    pose_stamped.pose =
        transform::Rigid3dToGeometryPose(node_pose.data.global_pose);
    path.poses.push_back(pose_stamped);
    // LOG(ERROR) << "pose:(" << node_pose.data.global_pose << ").";
  }
  node_poses_publisher_.publish(path);
}
void Slam::PublishSubmapPoses() {
  nav_msgs::Path path;
  path.header.stamp = ros::Time::now();
  path.header.frame_id = "map";
  mapping::MapById<mapping::SubmapId, mapping::SubmapPose> submap_poses =
      map_builder_ptr_->pose_graph()->GetAllSubmapPoses();
  for (auto&& submap_id_pose : submap_poses) {
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time::now();
    pose_stamped.header.frame_id = "map";
    pose_stamped.pose =
        transform::Rigid3dToGeometryPose(submap_id_pose.data.pose);
    path.poses.push_back(pose_stamped);
  }
  submap_poses_publisher_.publish(path);
}

void Slam::PublishSubmaps() {
  mapping::MapById<mapping::SubmapId, mapping::SubmapPose> submap_poses =
      map_builder_ptr_->pose_graph()->GetAllSubmapPoses();
  mapping::MapById<mapping::SubmapId, mapping::SubmapData> submap_data =
      map_builder_ptr_->pose_graph()->GetAllSubmapData();

  for (auto&& submap_id_pose : submap_poses) { 
      const mapping::ProbabilityGrid& probability_grid =
          static_cast<const mapping::ProbabilityGrid&>(
              *std::static_pointer_cast<const mapping::Submap2D>(
                    map_builder_ptr_->pose_graph()
                        ->GetSubmapData(submap_id_pose.id)
                        .submap)
                    ->grid());

      Eigen::Array2i offset;
      mapping::CellLimits cell_limits;
      probability_grid.ComputeCroppedLimits(&offset, &cell_limits);

      Eigen::Vector2f max(probability_grid.limits().max().y(),probability_grid.limits().max().x());  
      Eigen::Vector2f min(probability_grid.limits().max().y() - cell_limits.num_x_cells * kResolution,
        probability_grid.limits().max().x() - cell_limits.num_y_cells * kResolution);  
      transform::Rigid3d submap_pose = submap_id_pose.data.pose;
      Eigen::Vector2f submap_position(submap_pose.translation().x(),submap_pose.translation().y());
      transform::Rigid3d min_pose_to_submap_pose(
        Eigen::Vector3d(probability_grid.limits().max().x() - cell_limits.num_y_cells * kResolution-submap_pose.translation().x(),
        probability_grid.limits().max().y() - cell_limits.num_x_cells * kResolution-submap_pose.translation().y(),0),
        // submap_pose.rotation());
        Eigen::Quaterniond::Identity());
      transform::Rigid3d min_pose = submap_pose * min_pose_to_submap_pose;

      nav_msgs::OccupancyGrid occupancy_grid;
      occupancy_grid.header.stamp = ros::Time::now();
      occupancy_grid.header.frame_id = "map";
      occupancy_grid.info.resolution = kResolution;
      occupancy_grid.info.width = cell_limits.num_y_cells;
      occupancy_grid.info.height = cell_limits.num_x_cells;
      occupancy_grid.info.origin = transform::Rigid3dToGeometryPose(min_pose);

      for (int16_t x = cell_limits.num_x_cells -1; x >= 0; x--) {
        for (int16_t y = cell_limits.num_y_cells-1; y >= 0 ; y--) {
          Eigen::Array2i xy_index(x, y);
          if (!probability_grid.IsKnown(xy_index+offset)) {
            occupancy_grid.data.push_back(-1);
          } else {
            const int delta = 100 * probability_grid.GetProbability(xy_index+offset); 
            if (delta > 0)
              occupancy_grid.data.push_back(delta);
            else
              occupancy_grid.data.push_back(-1);
          }
        }
      }
      
      geometry_msgs::PoseStamped pose_stamped;
      pose_stamped.header.stamp = ros::Time::now();
      pose_stamped.header.frame_id = "map";
      pose_stamped.pose =
      transform::Rigid3dToGeometryPose(submap_id_pose.data.pose);
      submap_pose_publisher_.publish(pose_stamped);      
      pose_stamped.pose.position.x = min.y();
      pose_stamped.pose.position.y = min.x();
      submap_min_pose_publisher_.publish(pose_stamped);
      pose_stamped.pose = transform::Rigid3dToGeometryPose(min_pose);
      submap_max_pose_publisher_.publish(pose_stamped);
      submaps_publisher_.publish(occupancy_grid);
      // sleep(1);
  }
}

void Slam::PublishMap() {
  mapping::MapById<mapping::SubmapId, mapping::SubmapPose> submap_poses =
      map_builder_ptr_->pose_graph()->GetAllSubmapPoses();

  std::map<mapping::SubmapId, io::SubmapSlice> submap_slices;
  for (auto&& submap_id_pose : submap_poses) {
    mapping::proto::SubmapQuery::Response proto;
    const std::string error =
        map_builder_ptr_->SubmapToProto(submap_id_pose.id, &proto);
    auto& submap_slice = submap_slices[submap_id_pose.id];

    if (proto.textures().size() > 1) {
      LOG(ERROR) << "Proto textures size is (" << proto.textures().size()
                 << "),continue.";
      continue;
    }
    if (proto.textures().empty()) {
      LOG(ERROR) << "Proto textures is empty,continue.";
      continue;
    }

    // nav_msgs::OccupancyGrid grid;
    // grid.header.stamp = ros::Time::now();
    // grid.header.frame_id = "map";
    for (auto&& texture : proto.textures()) {
      submap_slice.width = texture.width();
      submap_slice.height = texture.height();
      submap_slice.resolution = texture.resolution();
      // submap_slice.slice_pose = texture.slice_pose;
      // texture.cells();
    }
  nav_msgs::OccupancyGrid occupancy_grid;
  occupancy_grid.header.stamp = ros::Time::now();
  occupancy_grid.header.frame_id = "map";
  occupancy_grid.info.resolution = kResolution;
  // fullmap_publisher_.pushlish(occupancy_grid);
  }
}

void Slam::PublishTrajectoryMap(const int& trajectory_id) {}

}  // namespace slam
}  // namespace cartographer